#include "ExchangeMatrix.h"
#include "common/constant.h"

AMCAX_EXCHANGE_NAMESPACE_BEGIN
    ExchangeMatrix::ExchangeMatrix(double orientation[3][3], double origin[3])
    {
        for (int i = 0; i < 3; ++i)
        {
            for (int j = 0; j < 3; ++j)
            {
                orientation_[i][j] = orientation[i][j];
            }
            origin_[i] = origin[i];
        }
    }

    std::array<std::array<double, 3>, 3> ExchangeMatrix::GetOrientation() const
    {
        return orientation_;
    }

    std::array<double, 3> ExchangeMatrix::GetOrigin() const
    {
        return origin_;
    }

    ExchangeMatrix ExchangeMatrix::LeftMultiply(const ExchangeMatrix &other) const
    {
        ExchangeMatrix result;
        for (int i = 0; i < 3; ++i)
        {
            for (int j = 0; j < 3; ++j)
            {
                result.orientation_[i][j] = 0;
                for (int k = 0; k < 3; ++k)
                {
                    result.orientation_[i][j] += orientation_[i][k] * other.orientation_[k][j];
                }
            }
            result.origin_[i] = origin_[i];
            for (int j = 0; j < 3; ++j)
            {
                result.origin_[i] += orientation_[i][j] * other.origin_[j];
            }
        }
        return result;   
    }

AMCAX_EXCHANGE_NAMESPACE_END